#ifndef __OCPP_Cesium_Quaternion_H__
#define __OCPP_Cesium_Quaternion_H__

#include "Spherical.h"
#include "Cartesian3.h"
#include "Matrix3.h"
#include "HeadingPitchRoll.h"

namespace OCPP
{
	namespace Cesium
	{
		class _CesiumExport Quaternion
		{
		public:
			double x, y, z, w;
			Quaternion(double _x = 0.0, double _y = 0.0, double _z = 0.0, double _w = 0.0)
				: x(_x)
				, y(_y)
				, z(_z)
				, w(_w)
			{

			}

			Quaternion& operator = (const Quaternion& fScaler)
			{
				return (Quaternion&)*this;
			}
	
			const static Quaternion ZERO;
			const static Quaternion IDENTITY;

			/**
			 * Computes a quaternion representing a rotation around an axis.
			 *
			 * @param {Cartesian3} axis The axis of rotation.
			 * @param {number} angle The angle in radians to rotate around the axis.
			 * @param {Quaternion} [result] The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter or a new Quaternion instance if one was not provided.
			 */
			static Quaternion fromAxisAngle(Cartesian3 axis, double angle) {
				auto halfAngle = angle / 2.0;
				auto s = sin(halfAngle);
				auto fromAxisAngleScratch = Cartesian3::normalize(axis);

				auto x = fromAxisAngleScratch.x * s;
				auto y = fromAxisAngleScratch.y * s;
				auto z = fromAxisAngleScratch.z * s;
				auto w = cos(halfAngle);
				Quaternion result;
				result.x = x;
				result.y = y;
				result.z = z;
				result.w = w;
				return result;
			};

			inline const static IntVector fromRotationMatrixNext = { 1, 2, 0 };
			inline const static IntVector fromRotationMatrixQuat = { 0, 0, 0 };
			/**
			 * Computes a Quaternion from the provided Matrix3 instance.
			 *
			 * @param {Matrix3} matrix The rotation matrix.
			 * @param {Quaternion} [result] The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter or a new Quaternion instance if one was not provided.
			 *
			 * @see Matrix3::fromQuaternion
			 */
			static Quaternion fromRotationMatrix(Matrix3 matrix) {
				double root;
				double x;
				double y;
				double z;
				double w;

				auto m00 = matrix[Matrix3::COLUMN0ROW0];
				auto m11 = matrix[Matrix3::COLUMN1ROW1];
				auto m22 = matrix[Matrix3::COLUMN2ROW2];
				auto trace = m00 + m11 + m22;

				if (trace > 0.0) {
					// |w| > 1/2, may as well choose w > 1/2
					root = sqrt(trace + 1.0); // 2w
					w = 0.5 * root;
					root = 0.5 / root; // 1/(4w)

					x = (matrix[Matrix3::COLUMN1ROW2] - matrix[Matrix3::COLUMN2ROW1]) * root;
					y = (matrix[Matrix3::COLUMN2ROW0] - matrix[Matrix3::COLUMN0ROW2]) * root;
					z = (matrix[Matrix3::COLUMN0ROW1] - matrix[Matrix3::COLUMN1ROW0]) * root;
				}
				else {
					// |w| <= 1/2
					auto next = fromRotationMatrixNext;

					auto i = 0;
					if (m11 > m00) {
						i = 1;
					}
					if (m22 > m00 && m22 > m11) {
						i = 2;
					}
					auto j = next[i];
					auto k = next[j];

					root = sqrt(
						matrix[Matrix3::getElementIndex(i, i)] -
						matrix[Matrix3::getElementIndex(j, j)] -
						matrix[Matrix3::getElementIndex(k, k)] +
						1.0
					);

					auto quat = fromRotationMatrixQuat;
					quat[i] = 0.5 * root;
					root = 0.5 / root;
					w =
						(matrix[Matrix3::getElementIndex(k, j)] -
							matrix[Matrix3::getElementIndex(j, k)]) *
						root;
					quat[j] =
						(matrix[Matrix3::getElementIndex(j, i)] +
							matrix[Matrix3::getElementIndex(i, j)]) *
						root;
					quat[k] =
						(matrix[Matrix3::getElementIndex(k, i)] +
							matrix[Matrix3::getElementIndex(i, k)]) *
						root;

					x = -quat[0];
					y = -quat[1];
					z = -quat[2];
				}

				return Quaternion(x, y, z, w);
			};

			/**
			 * Computes a rotation from the given heading, pitch and roll angles. Heading is the rotation about the
			 * negative z axis. Pitch is the rotation about the negative y axis. Roll is the rotation about
			 * the positive x axis.
			 *
			 * @param {HeadingPitchRoll} headingPitchRoll The rotation expressed as a heading, pitch and roll.
			 * @param {Quaternion} [result] The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter or a new Quaternion instance if none was provided.
			 */
			static Quaternion fromHeadingPitchRoll(HeadingPitchRoll headingPitchRoll) {
				auto scratchRollQuaternion = Quaternion::fromAxisAngle(
					Cartesian3::UNIT_X,
					headingPitchRoll.roll
				);
				auto scratchPitchQuaternion = Quaternion::fromAxisAngle(
					Cartesian3::UNIT_Y,
					-headingPitchRoll.pitch
				);
				auto result = Quaternion::multiply(
					scratchPitchQuaternion,
					scratchRollQuaternion
				);
				auto scratchHeadingQuaternion = Quaternion::fromAxisAngle(
					Cartesian3::UNIT_Z,
					-headingPitchRoll.heading
				);
				return Quaternion::multiply(scratchHeadingQuaternion, result);
			};

			/**
			 * The number of elements used to pack the object into an array1.
			 * @type {number}
			 */
			const static int packedLength = 4;

			/**
			 * Stores the provided instance into the provided array1.
			 *
			 * @param {Quaternion} value The value to pack.
			 * @param {number[]} array1 The array1 to pack into.
			 * @param {number} [startingIndex=0] The index into the array1 at which to start packing the elements.
			 *
			 * @returns {number[]} The array1 that was packed into
			 */
			DoubleVector pack(Quaternion value, DoubleVector array1, size_t startingIndex = 0) {
				array1[startingIndex++] = value.x;
				array1[startingIndex++] = value.y;
				array1[startingIndex++] = value.z;
				array1[startingIndex] = value.w;

				return array1;
			};

			/**
			 * Retrieves an instance from a packed array1.
			 *
			 * @param {number[]} array1 The packed array1.
			 * @param {number} [startingIndex=0] The starting index of the element to be unpacked.
			 * @param {Quaternion} [result] The object into which to store the result.
			 * @returns {Quaternion} The modified result parameter or a new Quaternion instance if one was not provided.
			 */
			static Quaternion unpack(DoubleVector array1, size_t startingIndex = 0) {
				Quaternion result;

				result.x = array1[startingIndex];
				result.y = array1[startingIndex + 1];
				result.z = array1[startingIndex + 2];
				result.w = array1[startingIndex + 3];
				return result;
			};

			/**
			 * The number of elements used to store the object into an array1 in its interpolatable form.
			 * @type {number}
			 */
			const static int packedInterpolationLength = 3;

			/**
			 * Converts a packed array1 into a form suitable for interpolation.
			 *
			 * @param {number[]} packedArray The packed array1.
			 * @param {number} [startingIndex=0] The index of the first element to be converted.
			 * @param {number} [lastIndex=packedArray.length] The index of the last element to be converted.
			 * @param {number[]} [result] The object into which to store the result.
			 */
			DoubleVector convertPackedArrayForInterpolation(
				DoubleVector packedArray,
				size_t startingIndex,
				size_t lastIndex
			) {
				DoubleVector result;
				auto sampledQuaternionQuaternion0Conjugate = Quaternion::unpack(
					packedArray,
					lastIndex * 4
				);
				Quaternion::conjugate(
					sampledQuaternionQuaternion0Conjugate
				);
				auto len = lastIndex - startingIndex + 1;
				for (auto i = 0; i < len; i++) {
					auto offset = i * 3;
					auto sampledQuaternionTempQuaternion = Quaternion::unpack(
						packedArray,
						(startingIndex + i) * 4
					);

					Quaternion::multiply(
						sampledQuaternionTempQuaternion,
						sampledQuaternionQuaternion0Conjugate
					);

					if (sampledQuaternionTempQuaternion.w < 0) {
						Quaternion::negate(
							sampledQuaternionTempQuaternion
						);
					}

					auto sampledQuaternionAxis = Quaternion::computeAxis(
						sampledQuaternionTempQuaternion
					);
					auto angle = Quaternion::computeAngle(sampledQuaternionTempQuaternion);
					result[offset] = sampledQuaternionAxis.x * angle;
					result[offset + 1] = sampledQuaternionAxis.y * angle;
					result[offset + 2] = sampledQuaternionAxis.z * angle;
				}

				return result;
			};

			/**
			 * Retrieves an instance from a packed array1 converted with {@link convertPackedArrayForInterpolation}.
			 *
			 * @param {number[]} array1 The array1 previously packed for interpolation.
			 * @param {number[]} sourceArray The original packed array1.
			 * @param {number} [firstIndex=0] The firstIndex used to convert the array1.
			 * @param {number} [lastIndex=packedArray.length] The lastIndex used to convert the array1.
			 * @param {Quaternion} [result] The object into which to store the result.
			 * @returns {Quaternion} The modified result parameter or a new Quaternion instance if one was not provided.
			 */
			static Quaternion unpackInterpolationResult(
				DoubleVector array1,
				DoubleVector sourceArray,
				size_t firstIndex,
				size_t lastIndex
			) {
				Quaternion result;
				auto sampledQuaternionRotation = Cartesian3::fromArray(array1, 0);
				auto magnitude = Cartesian3::magnitude(sampledQuaternionRotation);

				auto sampledQuaternionQuaternion0 = Quaternion::unpack(sourceArray, lastIndex * 4);

				Quaternion sampledQuaternionTempQuaternion;

				if (magnitude == 0) {
					sampledQuaternionTempQuaternion = Quaternion::IDENTITY;
				}
				else {
					sampledQuaternionTempQuaternion = Quaternion::fromAxisAngle(
						sampledQuaternionRotation,
						magnitude
					);
				}

				return Quaternion::multiply(
					sampledQuaternionTempQuaternion,
					sampledQuaternionQuaternion0
				);
			};

			/**
			 * Computes the conjugate of the provided quaternion.
			 *
			 * @param {Quaternion} quaternion The quaternion to conjugate.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion conjugate(Quaternion quaternion) {
				Quaternion result;

				result.x = -quaternion.x;
				result.y = -quaternion.y;
				result.z = -quaternion.z;
				result.w = quaternion.w;
				return result;
			};

			/**
			 * Computes magnitude squared for the provided quaternion.
			 *
			 * @param {Quaternion} quaternion The quaternion to conjugate.
			 * @returns {number} The magnitude squared.
			 */
			static double magnitudeSquared(Quaternion quaternion) {
				return (
					quaternion.x * quaternion.x +
					quaternion.y * quaternion.y +
					quaternion.z * quaternion.z +
					quaternion.w * quaternion.w
					);
			};

			/**
			 * Computes magnitude for the provided quaternion.
			 *
			 * @param {Quaternion} quaternion The quaternion to conjugate.
			 * @returns {number} The magnitude.
			 */
			static double magnitude(Quaternion quaternion) {
				return sqrt(Quaternion::magnitudeSquared(quaternion));
			};

			/**
			 * Computes the normalized form of the provided quaternion.
			 *
			 * @param {Quaternion} quaternion The quaternion to normalize.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion normalize(Quaternion quaternion) {
				Quaternion result;

				auto inverseMagnitude = 1.0 / Quaternion::magnitude(quaternion);
				auto x = quaternion.x * inverseMagnitude;
				auto y = quaternion.y * inverseMagnitude;
				auto z = quaternion.z * inverseMagnitude;
				auto w = quaternion.w * inverseMagnitude;

				result.x = x;
				result.y = y;
				result.z = z;
				result.w = w;
				return result;
			};

			/**
			 * Computes the inverse of the provided quaternion.
			 *
			 * @param {Quaternion} quaternion The quaternion to normalize.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion inverse(Quaternion quaternion) {
				Quaternion result;
				auto magnitudeSquared = Quaternion::magnitudeSquared(quaternion);
				result = Quaternion::conjugate(quaternion);
				return Quaternion::multiplyByScalar(result, 1.0 / magnitudeSquared);
			};

			/**
			 * Computes the componentwise sum of two quaternions.
			 *
			 * @param {Quaternion} left The first quaternion.
			 * @param {Quaternion} right The second quaternion.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion add(Quaternion left, Quaternion right) {
				Quaternion result;

				result.x = left.x + right.x;
				result.y = left.y + right.y;
				result.z = left.z + right.z;
				result.w = left.w + right.w;
				return result;
			};

			/**
			 * Computes the componentwise difference of two quaternions.
			 *
			 * @param {Quaternion} left The first quaternion.
			 * @param {Quaternion} right The second quaternion.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion subtract(Quaternion left, Quaternion right) {
				Quaternion result;

				result.x = left.x - right.x;
				result.y = left.y - right.y;
				result.z = left.z - right.z;
				result.w = left.w - right.w;
				return result;
			};

			/**
			 * Negates the provided quaternion.
			 *
			 * @param {Quaternion} quaternion The quaternion to be negated.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion negate(Quaternion quaternion) {
				Quaternion result;

				result.x = -quaternion.x;
				result.y = -quaternion.y;
				result.z = -quaternion.z;
				result.w = -quaternion.w;
				return result;
			};

			/**
			 * Computes the dot (scalar) product of two quaternions.
			 *
			 * @param {Quaternion} left The first quaternion.
			 * @param {Quaternion} right The second quaternion.
			 * @returns {number} The dot product.
			 */
			static double dot(Quaternion left, Quaternion right) {
				Quaternion result;

				return (
					left.x * right.x + left.y * right.y + left.z * right.z + left.w * right.w
					);
			};

			/**
			 * Computes the product of two quaternions.
			 *
			 * @param {Quaternion} left The first quaternion.
			 * @param {Quaternion} right The second quaternion.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion multiply(Quaternion left, Quaternion right) {
				Quaternion result;

				auto leftX = left.x;
				auto leftY = left.y;
				auto leftZ = left.z;
				auto leftW = left.w;

				auto rightX = right.x;
				auto rightY = right.y;
				auto rightZ = right.z;
				auto rightW = right.w;

				auto x = leftW * rightX + leftX * rightW + leftY * rightZ - leftZ * rightY;
				auto y = leftW * rightY - leftX * rightZ + leftY * rightW + leftZ * rightX;
				auto z = leftW * rightZ + leftX * rightY - leftY * rightX + leftZ * rightW;
				auto w = leftW * rightW - leftX * rightX - leftY * rightY - leftZ * rightZ;

				result.x = x;
				result.y = y;
				result.z = z;
				result.w = w;
				return result;
			};

			/**
			 * Multiplies the provided quaternion componentwise by the provided scalar.
			 *
			 * @param {Quaternion} quaternion The quaternion to be scaled.
			 * @param {number} scalar The scalar to multiply with.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion multiplyByScalar(Quaternion quaternion, double scalar) {
				Quaternion result;

				result.x = quaternion.x * scalar;
				result.y = quaternion.y * scalar;
				result.z = quaternion.z * scalar;
				result.w = quaternion.w * scalar;
				return result;
			};

			/**
			 * Divides the provided quaternion componentwise by the provided scalar.
			 *
			 * @param {Quaternion} quaternion The quaternion to be divided.
			 * @param {number} scalar The scalar to divide by.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion divideByScalar(Quaternion quaternion, double scalar) {
				Quaternion result;

				result.x = quaternion.x / scalar;
				result.y = quaternion.y / scalar;
				result.z = quaternion.z / scalar;
				result.w = quaternion.w / scalar;
				return result;
			};

			/**
			 * Computes the axis of rotation of the provided quaternion.
			 *
			 * @param {Quaternion} quaternion The quaternion to use.
			 * @param {Cartesian3} result The object onto which to store the result.
			 * @returns {Cartesian3} The modified result parameter.
			 */
			static Cartesian3 computeAxis(Quaternion quaternion) {
				Cartesian3 result;
				auto w = quaternion.w;
				if (
					std::abs(w - 1.0) < CesiumMath::EPSILON6 ||
					std::abs(w + 1.0) < CesiumMath::EPSILON6
					) {
					result.x = 1;
					result.y = result.z = 0;
					return result;
				}

				auto scalar = 1.0 / sqrt(1.0 - w * w);

				result.x = quaternion.x * scalar;
				result.y = quaternion.y * scalar;
				result.z = quaternion.z * scalar;
				return result;
			};

			/**
			 * Computes the angle of rotation of the provided quaternion.
			 *
			 * @param {Quaternion} quaternion The quaternion to use.
			 * @returns {number} The angle of rotation.
			 */
			static double computeAngle(Quaternion quaternion) {
				if (std::abs(quaternion.w - 1.0) < CesiumMath::EPSILON6) {
					return 0.0;
				}
				return 2.0 * acos(quaternion.w);
			};

			/**
			 * Computes the linear interpolation or extrapolation at t using the provided quaternions.
			 *
			 * @param {Quaternion} start The value corresponding to t at 0.0.
			 * @param {Quaternion} end The value corresponding to t at 1.0.
			 * @param {number} t The point along t at which to interpolate.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion lerp(Quaternion start, Quaternion end, double t) {
				Quaternion result;

				Quaternion lerpScratch = Quaternion::multiplyByScalar(end, t);
				result = Quaternion::multiplyByScalar(start, 1.0 - t);
				return Quaternion::add(lerpScratch, result);
			};

			/**
			 * Computes the spherical linear interpolation or extrapolation at t using the provided quaternions.
			 *
			 * @param {Quaternion} start The value corresponding to t at 0.0.
			 * @param {Quaternion} end The value corresponding to t at 1.0.
			 * @param {number} t The point along t at which to interpolate.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 *
			 * @see Quaternion#fastSlerp
			 */
			static Quaternion slerp(Quaternion start, Quaternion end, double t) {
				auto dot = Quaternion::dot(start, end);

				// The angle between start must be acute. Since q and -q represent
				// the same rotation, negate q to get the acute angle.
				Quaternion r = end;
				Quaternion slerpEndNegated;
				if (dot < 0.0) {
					dot = -dot;
					r = slerpEndNegated = Quaternion::negate(end);
				}

				// dot > 0, as the dot product approaches 1, the angle between the
				// quaternions vanishes. use linear interpolation.
				if (1.0 - dot < CesiumMath::EPSILON6) {
					return Quaternion::lerp(start, r, t);
				}

				auto theta = acos(dot);
				Quaternion slerpScaledP = Quaternion::multiplyByScalar(
					start,
					sin((1 - t) * theta)
				);
				Quaternion slerpScaledR = Quaternion::multiplyByScalar(
					r,
					sin(t * theta)
				);
				Quaternion result = Quaternion::add(slerpScaledP, slerpScaledR);
				return Quaternion::multiplyByScalar(result, 1.0 / sin(theta));
			};

			/**
			 * The logarithmic quaternion function.
			 *
			 * @param {Quaternion} quaternion The unit quaternion.
			 * @param {Cartesian3} result The object onto which to store the result.
			 * @returns {Cartesian3} The modified result parameter.
			 */
			static Cartesian3 log(Quaternion quaternion) {
				auto theta = CesiumMath::acosClamped(quaternion.w);
				auto thetaOverSinTheta = 0.0;

				if (theta != 0.0) {
					thetaOverSinTheta = theta / sin(theta);
				}

				return Cartesian3::multiplyByScalar(quaternion, thetaOverSinTheta);
			};

			/**
			 * The exponential quaternion function.
			 *
			 * @param {Cartesian3} cartesian The cartesian.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 */
			static Quaternion exp(Cartesian3 cartesian) {
				auto theta = Cartesian3::magnitude(cartesian);
				auto sinThetaOverTheta = 0.0;

				if (theta != 0.0) {
					sinThetaOverTheta = sin(theta) / theta;
				}
				Quaternion result;
				result.x = cartesian.x * sinThetaOverTheta;
				result.y = cartesian.y * sinThetaOverTheta;
				result.z = cartesian.z * sinThetaOverTheta;
				result.w = cos(theta);

				return result;
			};

			/**
			 * Computes an inner quadrangle point.
			 * <p>This will compute quaternions that ensure a squad curve is C<sup>1</sup>.</p>
			 *
			 * @param {Quaternion} q0 The first quaternion.
			 * @param {Quaternion} q1 The second quaternion.
			 * @param {Quaternion} q2 The third quaternion.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 *
			 * @see Quaternion#squad
			 */
			static Quaternion computeInnerQuadrangle(Quaternion q0, Quaternion q1, Quaternion q2) {
				Quaternion result;
				auto qInv = Quaternion::conjugate(q1);
				auto squadScratchQuaternion1 = Quaternion::multiply(qInv, q2);
				auto cart0 = Quaternion::log(squadScratchQuaternion1);

				Quaternion::multiply(qInv, q0);
				auto cart1 = Quaternion::log(squadScratchQuaternion1);

				cart0 = Cartesian3::add(cart0, cart1);
				cart0 = Cartesian3::multiplyByScalar(cart0, 0.25);
				cart0 = Cartesian3::negate(cart0);
				auto squadScratchQuaternion0 = Quaternion::exp(cart0);

				return Quaternion::multiply(q1, squadScratchQuaternion0);
			};

			/**
			 * Computes the spherical quadrangle interpolation between quaternions.
			 *
			 * @param {Quaternion} q0 The first quaternion.
			 * @param {Quaternion} q1 The second quaternion.
			 * @param {Quaternion} s0 The first inner quadrangle.
			 * @param {Quaternion} s1 The second inner quadrangle.
			 * @param {number} t The time in [0,1] used to interpolate.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 *
			 *
			 * @example
			 * // 1. compute the squad interpolation between two quaternions on a curve
			 * const s0 = Cesium.Quaternion::computeInnerQuadrangle(quaternions[i - 1], quaternions[i], quaternions[i + 1], new Cesium.Quaternion());
			 * const s1 = Cesium.Quaternion::computeInnerQuadrangle(quaternions[i], quaternions[i + 1], quaternions[i + 2], new Cesium.Quaternion());
			 * const q = Cesium.Quaternion::squad(quaternions[i], quaternions[i + 1], s0, s1, t, new Cesium.Quaternion());
			 *
			 * // 2. compute the squad interpolation as above but where the first quaternion is a end point.
			 * const s1 = Cesium.Quaternion::computeInnerQuadrangle(quaternions[0], quaternions[1], quaternions[2], new Cesium.Quaternion());
			 * const q = Cesium.Quaternion::squad(quaternions[0], quaternions[1], quaternions[0], s1, t, new Cesium.Quaternion());
			 *
			 * @see Quaternion#computeInnerQuadrangle
			 */
			static Quaternion squad(Quaternion q0, Quaternion q1, Quaternion s0, Quaternion s1, double t) {
				auto slerp0 = Quaternion::slerp(q0, q1, t);
				auto slerp1 = Quaternion::slerp(s0, s1, t);
				return Quaternion::slerp(slerp0, slerp1, 2.0 * t * (1.0 - t));
			};

			

			/**
			 * Computes the spherical linear interpolation or extrapolation at t using the provided quaternions.
			 * This implementation is faster than {@link Quaternion#slerp}, but is only accurate up to 10<sup>-6</sup>.
			 *
			 * @param {Quaternion} start The value corresponding to t at 0.0.
			 * @param {Quaternion} end The value corresponding to t at 1.0.
			 * @param {number} t The point along t at which to interpolate.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter.
			 *
			 * @see Quaternion#slerp
			 */
			static Quaternion fastSlerp(Quaternion start, Quaternion end, double t) {
				double opmu = 1.90110745351730037;
				DoubleVector u = { 0,0,0,0,0,0,0,0 };
				DoubleVector v = { 0,0,0,0,0,0,0,0 };
				DoubleVector bT = { 0,0,0,0,0,0,0,0 };
				DoubleVector bD = { 0,0,0,0,0,0,0,0 };

				for (auto i = 0; i < 7; ++i) {
					auto s = i + 1.0;
					auto t = 2.0 * s + 1.0;
					u[i] = 1.0 / (s * t);
					v[i] = s / t;
				}

				u[7] = opmu / (8.0 * 17.0);
				v[7] = (opmu * 8.0) / 17.0;

				auto x = Quaternion::dot(start, end);

				double sign;
				if (x >= 0) {
					sign = 1.0;
				}
				else {
					sign = -1.0;
					x = -x;
				}

				double xm1 = x - 1.0;
				double d = 1.0 - t;
				double sqrT = t * t;
				double sqrD = d * d;

				for (auto i = 7; i >= 0; --i) {
					bT[i] = (u[i] * sqrT - v[i]) * xm1;
					bD[i] = (u[i] * sqrD - v[i]) * xm1;
				}

				double cT =
					sign *
					t *
					(1.0 +
						bT[0] *
						(1.0 +
							bT[1] *
							(1.0 +
								bT[2] *
								(1.0 +
									bT[3] *
									(1.0 +
										bT[4] *
										(1.0 + bT[5] * (1.0 + bT[6] * (1.0 + bT[7]))))))));
				double cD =
					d *
					(1.0 +
						bD[0] *
						(1.0 +
							bD[1] *
							(1.0 +
								bD[2] *
								(1.0 +
									bD[3] *
									(1.0 +
										bD[4] *
										(1.0 + bD[5] * (1.0 + bD[6] * (1.0 + bD[7]))))))));

				auto temp = Quaternion::multiplyByScalar(
					start,
					cD
				);
				auto result = Quaternion::multiplyByScalar(end, cT);
				return Quaternion::add(temp, result);
			};

			/**
			 * Computes the spherical quadrangle interpolation between quaternions.
			 * An implementation that is faster than {@link Quaternion#squad}, but less accurate.
			 *
			 * @param {Quaternion} q0 The first quaternion.
			 * @param {Quaternion} q1 The second quaternion.
			 * @param {Quaternion} s0 The first inner quadrangle.
			 * @param {Quaternion} s1 The second inner quadrangle.
			 * @param {number} t The time in [0,1] used to interpolate.
			 * @param {Quaternion} result The object onto which to store the result.
			 * @returns {Quaternion} The modified result parameter or a new instance if none was provided.
			 *
			 * @see Quaternion#squad
			 */
			static Quaternion fastSquad(Quaternion q0, Quaternion  q1, Quaternion  s0, Quaternion s1,double t) {
				auto slerp0 = Quaternion::fastSlerp(q0, q1, t);
				auto slerp1 = Quaternion::fastSlerp(s0, s1, t);
				return Quaternion::fastSlerp(slerp0, slerp1, 2.0 * t * (1.0 - t));
			};

			static bool equalsEpsilon(Quaternion left, Quaternion right, double epsilon = 0.0) {
				return (
					std::abs(left.x - right.x) <= epsilon &&
					std::abs(left.y - right.y) <= epsilon &&
					std::abs(left.z - right.z) <= epsilon &&
					std::abs(left.w - right.w) <= epsilon
					);
			};

			static bool equals(Quaternion left, Quaternion right) {
				return (
					left.x == right.x &&
					left.y == right.y &&
					left.z == right.z &&
					left.w == right.w
					);
			};

			/**
			 * Compares this Cartesian against the provided Cartesian componentwise and returns
			 * <code>true</code> if they are equal, <code>false</code> otherwise.
			 *
			 * @param {Quaternion} [right] The right hand side Cartesian.
			 * @returns {boolean} <code>true</code> if they are equal, <code>false</code> otherwise.
			 */
			bool equals(Quaternion& right) {
				return Quaternion::equals(*this, right);
			};

			/**
			 * Compares this Cartesian against the provided Cartesian componentwise and returns
			 * <code>true</code> if they pass an absolute or relative tolerance test,
			 * <code>false</code> otherwise.
			 *
			 * @param {Quaternion} [right] The right hand side Cartesian.
			 * @param {number} [relativeEpsilon=0] The relative epsilon tolerance to use for equality testing.
			 * @param {number} [absoluteEpsilon=relativeEpsilon] The absolute epsilon tolerance to use for equality testing.
			 * @returns {boolean} <code>true</code> if they are within the provided epsilon, <code>false</code> otherwise.
			 */
			bool equalsEpsilon(
				Quaternion right,
				double epsilon = 0.0
			) {
				return Quaternion::equalsEpsilon(
					*this,
					right,
					epsilon
				);
			};

			/**
			 * Creates a string representing this Cartesian in the format '(x, y, z, w)'.
			 *
			 * @returns {string} A string representing the provided Cartesian in the format '(x, y, z, w)'.
			 */
			String toString() {

				return str_format("%f,%f,%f,%f", x, y, z, w);
			};

			// scratchU8Array and scratchF32Array are views into the same buffer
			//Uint8Vector scratchU8Array = new Uint8Array(scratchF32Array.buffer);
			//Uint32Vector testU32 = { 0x11223344 };
			//Uint8Vector testU8 = new Uint8Array(testU32.buffer);
			//const static bool littleEndian = testU8[0] == 0x44;
			const static bool littleEndian = true;

			/**
			 * Packs an arbitrary floating point value to 4 values representable using uint8.
			 *
			 * @param {number} value A floating point number.
			 * @param {Quaternion} [result] The Quaternion that will contain the packed float.
			 * @returns {Quaternion} A Quaternion representing the float packed to values in x, y, z, and w.
			 */
			static Quaternion packFloat(float value) {
				Quaternion result;

				std::vector<float> scratchF32Array = { value };

				Uint8Vector scratchU8Array;
				scratchU8Array.resize(4);

				memcpy(&scratchU8Array[0], &scratchF32Array[0], 4);

				//Uint8Vector scratchU8Array = new Uint8Array(scratchF32Array.buffer);
				if (littleEndian) {
					result.x = scratchU8Array[0];
					result.y = scratchU8Array[1];
					result.z = scratchU8Array[2];
					result.w = scratchU8Array[3];
				}
				else {
					// convert from big-endian to little-endian
					result.x = scratchU8Array[3];
					result.y = scratchU8Array[2];
					result.z = scratchU8Array[1];
					result.w = scratchU8Array[0];
				}
				return result;
			};

			/**
			 * Unpacks a float packed using Quaternion::packFloat.
			 *
			 * @param {Quaternion} packedFloat A Quaternion containing a float packed to 4 values representable using uint8.
			 * @returns {number} The unpacked float.
			 * @private
			 */
			static Quaternion unpackFloat(Quaternion packedFloat) {

				Uint8Vector scratchU8Array;
				scratchU8Array.resize(4);



				// scratchU8Array and scratchF32Array are views into the same buffer
				if (littleEndian) {
					scratchU8Array[0] = packedFloat.x;
					scratchU8Array[1] = packedFloat.y;
					scratchU8Array[2] = packedFloat.z;
					scratchU8Array[3] = packedFloat.w;
				}
				else {
					// convert from little-endian to big-endian
					scratchU8Array[0] = packedFloat.w;
					scratchU8Array[1] = packedFloat.z;
					scratchU8Array[2] = packedFloat.y;
					scratchU8Array[3] = packedFloat.x;
				}

				std::vector<float> scratchF32Array = { 0.0 };
				memcpy(&scratchF32Array[0], &scratchU8Array[0], 4);

				return scratchF32Array[0];
			};
		};
	}
}

#endif

